#include "stm32f10x.h"
#include "Servo.h"                     // 舵机头文件
#include "serial.h"                    // 串口头文件
#include "OLED.h"
#include <stdlib.h>	
#include "Delay.h"
#include "PWM.h"
#include "LED.h"

float Bias_x;
float Bias_y;
float Pwm_x;       
float Pwm_y;
float angle_y=1400;
float angle_x=1400;
float last_angle_y=1400;
float last_angle_x=1400;

void PWM_Init(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//使能APB2时钟
	
	TIM_InternalClockConfig(TIM2);
	GPIO_InitTypeDef GPIO_InitStructure;//设定GPIO结构体
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;//AFIO复用 
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_1|GPIO_Pin_2;//Pin
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStructure);//将设置的结构体的值传入
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;//第三步时基单元初始化
	TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;//向上计数
	TIM_TimeBaseInitStruct.TIM_Period=20000-1;//周期,ARR重装计数器的值，取值0~65535
	TIM_TimeBaseInitStruct.TIM_Prescaler=72-1;//PSC预分频器的值,因为预分频器和计数器都有一个数的偏差，所以-1
	TIM_TimeBaseInitStruct.TIM_RepetitionCounter=0;//重复计数器的值，高级计数器才有
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
	
	TIM_OCInitTypeDef TIM_OCInitStruct;//初始化输出比较单元
	TIM_OCStructInit(&TIM_OCInitStruct);
	TIM_OCInitStruct.TIM_OCMode=TIM_OCMode_PWM1; 
	TIM_OCInitStruct.TIM_OCPolarity=TIM_OCPolarity_High;//设置极性
	TIM_OCInitStruct.TIM_OutputState=TIM_OutputState_Enable;//输出使能
	TIM_OCInitStruct.TIM_Pulse=0;//CCR
	TIM_OC2Init(TIM2,&TIM_OCInitStruct);//通道二
	TIM_OC3Init(TIM2,&TIM_OCInitStruct);//通道三
	
	TIM_Cmd(TIM2,ENABLE);//启动定时器
}

void PWM_SetCompare1(uint16_t Compare1)//通道一
{
	TIM_SetCompare1(TIM2,Compare1);//0~ARR
}
void PWM_SetCompare2(uint16_t Compare)//通道二	
{
	TIM_SetCompare2(TIM2,Compare);//0~ARR
}
void PWM_SetCompare3(uint16_t Compare3)//通道三
{
	TIM_SetCompare3(TIM2,Compare3);//0~ARR
}

void PWM_SetPrescaler(uint16_t Prescaler)
{
	TIM_PrescalerConfig(TIM2,Prescaler,TIM_PSCReloadMode_Immediate);
}
 
 
 void Servo_Init(void)
{
	PWM_Init();									//初始化舵机的底层PWM
}

void Servo_SetAngle1(float Angle_y)           //angle_y范围0度到270度        Y轴
{
     PWM_SetCompare2(Angle_y / 2700 * 2000 + 500);                
}
 
void Servo_SetAngle2(float Angle_x)           //X轴
{	
     PWM_SetCompare3(Angle_x / 2700 * 2000 + 500);
}
 
// PID 控制舵机 1，用于 X 轴舵机的控制
// 传入参数：目标 X 值（Red_x）和实际 X 值（Green_x）
float Position_PIDx(float Red_x, float Green_x)
{
    float Position_KP = 0.2, Position_KI = 0.001, Position_KD = 0.1; // PID 控制参数：比例、积分、微分系数
    float Bias_x, Pwm_x, Integral_bias_x, Last_Bias_x; // 定义偏差、PWM 控制信号、积分偏差和上次偏差变量

    // 计算当前偏差（目标位置与实际位置之差）
    Bias_x = Green_x - Red_x;

    // 累积偏差，计算积分项
    Integral_bias_x += Bias_x;

    // 计算 PWM 控制信号，使用位置式 PID 控制公式
    Pwm_x = Position_KP * Bias_x + Position_KI * Integral_bias_x + Position_KD * (Bias_x - Last_Bias_x);

    // 保存当前偏差为上次偏差，供下一次计算微分项使用
    Last_Bias_x = Bias_x;

    // 对 PWM 控制信号进行限幅处理，防止过大值影响舵机控制
    if (Pwm_x > 11)
        Pwm_x = 10;
    if (Pwm_x < -11)
        Pwm_x = -10;

    // 对积分项进行限幅，防止积分饱和
    if (Integral_bias_x >= 5)
        Integral_bias_x = 5;
    if (Integral_bias_x <= -5)
        Integral_bias_x = -5;

    // 返回计算得到的 PWM 控制信号值
    return Pwm_x;
}

// PID 控制舵机 2，用于 Y 轴舵机的控制
// 传入参数：目标 Y 值（Red_y）和实际 Y 值（Green_y）
float Position_PIDy(float Red_y, float Green_y)
{
    float Position_KP = 0.2, Position_KI = 0.001, Position_KD = 0.1; // PID 控制参数：比例、积分、微分系数
    float Bias_y, Pwm_y, Integral_bias_y, Last_Bias_y; // 定义偏差、PWM 控制信号、积分偏差和上次偏差变量

    // 计算当前偏差（目标位置与实际位置之差）
    Bias_y = Green_y - Red_y;

    // 累积偏差，计算积分项
    Integral_bias_y += Bias_y;

    // 计算 PWM 控制信号，使用位置式 PID 控制公式
    Pwm_y = Position_KP * Bias_y + Position_KI * Integral_bias_y + Position_KD * (Bias_y - Last_Bias_y);

    // 保存当前偏差为上次偏差，供下一次计算微分项使用
    Last_Bias_y = Bias_y;

    // 对 PWM 控制信号进行限幅处理，防止过大值影响舵机控制
    if (Pwm_y > 5)        Pwm_y = 4;
    if (Pwm_y < -5)       Pwm_y = -4;

    // 对积分项进行限幅，防止积分饱和
    if (Integral_bias_y >= 5)      Integral_bias_y = 5;
    if (Integral_bias_y <= -5)		Integral_bias_y = -5;

    // 返回计算得到的 PWM 控制信号值
    return Pwm_y;
}

// PID 任务执行函数，用于周期性执行 PID 控制算法
void PID_Task_Execute(void)
{
    // 关闭 LED 输出
    LED_OFF();
    // 对 Y 轴舵机执行 PID 控制，获取 PWM 控制信号
    Pwm_y = Position_PIDy(Red_y, Green_y);
    // 根据 PWM 控制信号调整 Y 轴舵机的角度（不断累加的方式）
    angle_y = angle_y + Pwm_y;
    // 保存当前角度为上一次角度，方便后续操作
    last_angle_y = angle_y;
    // 对舵机角度进行限幅，防止超出舵机可转动范围
    if (angle_y >= 1800)      angle_y = 1800;
    if (angle_y <= 1200)    angle_y = 1200;
    // 设置 Y 轴舵机的实际角度
    Servo_SetAngle1(angle_y);
    // 对 X 轴舵机执行 PID 控制，获取 PWM 控制信号
    Pwm_x = Position_PIDx(Red_x, Green_x);
    // 根据 PWM 控制信号调整 X 轴舵机的角度（不断累加的方式）
    angle_x = angle_x + Pwm_x;
    // 保存当前角度为上一次角度，方便后续操作
    last_angle_x = angle_x;
	// 设置 X 轴舵机的实际角度
    Servo_SetAngle2(angle_x);
    // 对舵机角度进行限幅，防止超出舵机可转动范围
    if (angle_x >= 1800)        angle_x = 1800;
    if (angle_x <= 1200)        angle_x = 1200;
   
    // 检查是否达到目标点，当实际位置与目标位置偏差在合理范围内时，认为达到目标
    if (abs((int16_t)(Green_y - Red_y)) < 2 && abs((int16_t)(Green_y - Red_y)) < 2) {
        // 保持当前角度，停止进一步调整
        angle_y = last_angle_y;
        angle_x = last_angle_x;
        // 开启 LED 输出，指示已达到目标位置
        LED_ON();
    }
}

// PID 停止函数，用于停止舵机的 PID 控制调整
void PID_stop(void)
{
    // 将舵机角度恢复到上一次保存的角度值，使舵机停止在当前位置
    angle_y = last_angle_y;
    angle_x = last_angle_x;
}

// PID 复位函数，用于将舵机恢复到初始位置
void PID_restoration(void)
{
    // 将 Y 轴舵机角度复位到初始值（1400 对应中间位置）
    angle_y = 1400;
    Servo_SetAngle1(angle_y);

    // 将 X 轴舵机角度复位到初始值（1400 对应中间位置）
    angle_x = 1400;
    Servo_SetAngle2(angle_x);
}
